package com.grt192.controller.cannonbot;

import org.json.me.JSONException;
import org.json.me.JSONObject;

import com.grt192.core.EventController;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.mechanism.cannonbot.CBArm;
import com.grt192.mechanism.cannonbot.CBCannon;
import com.grt192.mechanism.cannonbot.CBTankDriveTrain;
import com.grt192.networking.GRTSingleClientServer;
import com.grt192.networking.SocketEvent;
import com.grt192.networking.SocketListener;

/**
 * 
 * @author anand
 * 
 */
public class CBRPGController extends EventController implements SocketListener {
	// Mechanisms
	private GRTDriverStation ds;
	private CBTankDriveTrain driveTrain;
	private CBCannon cannon;
	private CBArm arm;
	private GRTSingleClientServer server;

	public CBRPGController(GRTDriverStation ds, CBTankDriveTrain dt,
			CBCannon cannon, CBArm arm, int port) {
		this.ds = ds;
		this.driveTrain = dt;
		this.arm = arm;
		this.cannon = cannon;
		server = new GRTSingleClientServer(port);
		server.addSocketListener(this);
		server.start();
	}

	/**
	 * When data arrives on the socket, we parse it to JSON and then send the
	 * resulting data structure to be interpreted for use.
	 */
	public void dataRecieved(SocketEvent e) {
		try {
			JSONObject command = new JSONObject(e.getData());
			// parsing JSON can throw json exceptions.
			doCommand(command);
		} catch (Exception e1) {
			e1.printStackTrace();
		}
	}

	public void onConnect(SocketEvent e) {
		// TODO Auto-generated method stub

	}

	public void onDisconnect(SocketEvent e) {
		// TODO Auto-generated method stub

	}

	/**
	 * Given a JSONObject (essentially a hashtable) compare the "command" value
	 * to possible commandtypes... if its one of the commands, extract the
	 * appropriate parameters.
	 * 
	 * @throws JSONException
	 */
	protected void doCommand(JSONObject command) throws JSONException {
		if (command.getString("command").equals("moveto")) {
			double r = Double.parseDouble(command.getString("r"));
			double theta = Double.parseDouble(command.getString("theta"));
			double speed = Double.parseDouble(command.getString("speed"));
			drive(r, theta, speed);
		}
		if (command.getString("command").equals("arm")) {
			// toggle the arm
			this.arm.toggleArm();
		}
		if (command.getString("command").equals("fire")) {
			// fire the cannon
			this.cannon.fire();
		}
		if (command.getString("command").equals("stop")) {
			// stop DTs
			this.driveTrain.stop();
		}
		if (command.getString("command").equals("getstatus")) {
			JSONObject statusData = new JSONObject();
			// Put data into object..
			statusData.put("x", "" + driveTrain.getX());
			statusData.put("y", "" + driveTrain.getY());
			statusData.put("heading", "" + driveTrain.getHeading());
			statusData.put("arm", "" + arm.isExtended());
			statusData.put("firing", "" + cannon.isFiring());
			statusData.put("ready", "" + cannon.readyToFire());
			
			// Serialize it
			server.sendData(statusData.toString() + "\n");
		}
	}

	/**
	 * This takes a distance and angle at which a target point is located, and
	 * issues appropriate drive and turn commands to get there. uses
	 * http://code.
	 * google.com/p/grt192/source/browse/trunk/CannonBot/src/com/grt192
	 * /mechanism/cannonbot/CBTankDriveTrain.java
	 */
	protected void drive(double distance, double angle, double speed,
			boolean pointTurn) {
		if (pointTurn) {
			driveTrain.turnTo(angle, pointTurn);
			driveTrain.driveDistance(distance);
		}
	}

	protected void drive(double distance, double angle, double speed) {
		drive(distance, angle, speed, true);
	}
}
